Direct Collocation Methods for Trajectory Optimization in Constrained Robotic Systems

Direct collocation methods are powerful tools to solve trajectory optimization problems in robotics. While their resulting trajectories tend be dynamically accurate, they may also present large kinematic errors the case of constrained mechanical systems, i.e., those whose state coordinates subject holonomic or nonholonomic constraints, like loop-closure rolling-contact constraints. These constraints confine robot an implicitly-defined manifold, which complicates computation accurate solutions. Discretization inherent transcription problem easily make drift away from this results physically inconsistent motions that difficult track with a controller. This paper reviews existing deal and proposes new ones overcome limitations. Current approaches either disregard (which leads accumulation) modify system dynamics keep close manifold adds artificial forces energy dissipation system). The we propose, contrast, achieve full elimination on discrete trajectory, even along continuous one, without modifications dynamics. We illustrate compare using various examples different complexity.